import numpy as np

# 靶标坐标系，默认和相机系重合
# 点从左至右,单位m
plane_real_ptL = np.array([
    [-0.50, 0.00, 0],
    [-0.20,-0.10, 0],
    [ 0.00, 0.00, 0],
    [ 0.20,-0.10, 0],
    [ 0.50, 0.00, 0],
])
# 世界系为ENU则有靶标系至世界系的转换矩阵
R_wt = np.array([
    [0,0,1],
    [-1,0,0],
    [0,-1,0],
])



# 视场角约14.61
camK_lf = np.array([
    [4976.1875, 0.0000,  639.1333],
    [0.0000, 4990.3500,  512.1949],
    [0.0000,  0.0000,  1.0000],
])

# camK_lf = np.array([
#     [5250.2552, 0.0000,  639.1333],
#     [0.0000, 4200.2042,  512.1949],
#     [0.0000,  0.0000,  1.0000],
# ])

# camK_lf = np.array([
#     [7179.4351, 0.0000,  639.1333],
#     [0.0000, 6056.4614,  512.1949],
#     [0.0000,  0.0000,  1.0000],
# ])

# 视场角约110度
camK_sf = np.array([
    [ 441.7348,  0.0000,  640.2314],
    [ 0.0000,  441.5449,  512.8424],
    [ 0.0000,  0.0000,  1.0000],
])

# 相机系到FLU机体系的旋转矩阵
R_bc = np.array([
    [0,0,1],
    [-1,0,0],
    [0,-1,0],
])
R_cb = R_bc.T